/**
* @file distance.c
 *
 * @section desc File description
 *
 * @section copyright Copyright
 *
 * Trampoline is copyright (c) IRCCyN 2005-2007
 * Trampoline is protected by the French intellectual property law.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; version 2
 * of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 * @section infos File informations
 *
 * $Date$ 2013 04 18
 * $Rev$ 1
 * $Author$ Benjamin Sientzoff
 * $URL$
 */


/* Libraries */
#include "tpl_os.h"
#include "nxt_motors.h"
#include "ecrobot_interface.h"
#include "ecrobot_private.h"
#include "com_protocol.h"

SINT bluetooth_status = BT_NO_INIT;
boolean bt_connected = FALSE;

boolean prog_started  = FALSE;

extern U8 bt_frame[128];
extern boolean bt_frame_received;
//TODO: set bt_send_buffer size matching with max data send
U8 bt_mail[128] = {0, 0, 0, 0};
U8 Speed_motor = 0;
U8 chunk[256];
S16 dist_data[1];

U32 ultrasonicsensorvalue=0;


// What is the length of a string ?
/* strlen function from string.h */
U8 stringlength(char* s) 
{
		/* find length of s[] */
		U8 i = 0;
		while (s[i] != '\0'){
				i++;
		}
		return i;
}
/* Prototypes */
void init_genericI2C_sensor(U8 port_id);
void get_mindsensor_distance_sensor(U8 port_id, S16 *buf);
void term_genericI2C_sensor(U8 port_id);
void update_distance();


void update_distance() {
		get_mindsensor_distance_sensor(NXT_PORT_S3, dist_data);
		systick_wait_ms(2000);

}


void init_genericI2C_sensor(U8 port_id)
{
	nxt_avr_set_input_power(port_id,2);
	i2c_enable(port_id);
}

void get_mindsensor_distance_sensor(U8 port_id, S16 *buf)
{
    static S16 distance_state[0];
    static U8 data[2] = {0,0};
    
	if (i2c_busy(port_id) == 0) /* check the status of I2C comm. */
	{
		
        distance_state[0] = ((S16)data[1]<<8) + (S16)data[0];
		i2c_start_transaction(port_id,1,0x42,1,data,9,0);
	}
	
    buf[0] = distance_state[0];
    
	
}

void term_genericI2C_sensor(U8 port_id)
{
	i2c_disable(port_id);
}


void print(char* statement)
{

		display_goto_xy(0,7);
		display_string(statement);
		display_update();
}



FUNC(int,OS_APPL_CODE) main(void)
{            
		StartOS(OSDEFAULTAPPMODE);
	
		return 0;
}
FUNC(void,OS_APPL_CODE) StartupHook(void)
{
	init_genericI2C_sensor(NXT_PORT_S1);
}

FUNC(void,OS_APPL_CODE) ShutdownHook(StatusType error)
{
	term_genericI2C_sensor(NXT_PORT_S1);

}

/* TASKS */

TASK(init)
{
		
		TerminateTask();
}
TASK(majDistance){
	
	bluetooth_status = ecrobot_get_bt_status();
	update_distance();	

	
	TerminateTask();
}
//Display several informations about system,motors and sensors
TASK(display)
{
	
	static U8 TouchSensorStatus_old = 0;
	U8 TouchSensorStatus; 
		display_clear(0);
		//Battery stat
   	display_goto_xy(0,0);
   	display_string("RAQUETTE");
				
//				display_int(bt_frame[3],3);
		//ActivateTask(majDistance);
	
	//raquette use
		
		

	display_goto_xy(0,1);
   	display_string("S2");
	display_goto_xy(4,1);
	display_int(ecrobot_get_touch_sensor(NXT_PORT_S2),2);
	display_goto_xy(0,2);
   	display_string("S3");
	display_goto_xy(4,2);
	display_int(ecrobot_get_touch_sensor(NXT_PORT_S3),2);

		
		//sonar
		
		ultrasonicsensorvalue = ecrobot_get_sonar_sensor(NXT_PORT_S1);
		/*systick_wait_ms(100);
		ultrasonicsensorvalue = ecrobot_get_sonar_sensor(NXT_PORT_S1);
		systick_wait_ms(100);
		ultrasonicsensorvalue = ecrobot_get_sonar_sensor(NXT_PORT_S1);
		*/
		systick_wait_ms(100);
	display_goto_xy(0,3);
	display_string("SON");
	display_goto_xy(4,3);
	display_int(ultrasonicsensorvalue,4);
		
	
	
	
	display_update();
	TerminateTask();


}

//Send data throught bluetooth connection
TASK(postman)
{
		TerminateTask();
}

/* ISRs */
ISR(isr_button_stop)
{
    ShutdownOS(E_OK);
}

ISR(isr_button_left){
	
//	if ( !prog_started){
		prog_started=TRUE;
		Speed_motor=-100;
//	}
//	else {
//		prog_started=FALSE;
//		Speed_motor=0;
//	}
}
ISR(isr_button_start)
{
	

//		ActivateTask(display);
}

ISR(isr_bluetooth)
{
	// Moteur
	
	
	
}